﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Drifting.MotionPlanning.Systems
{
    public class RallyCar : BaseSystem
    {
        double c1 = 1;
        double c2 = 1;
        double mu1 = -10;
        double mu2 = -10;
        double f1_max = 6;
        double f2_max = 6;
        double M = 1;
        double J = 1;

        public RallyCar()
            : base("../../../MotionPlanning/Resources/RallyCar.kdl")
        {
            t_delta = 0.1;
        }

        public override void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            //q[0] = x
            //q[1] = y
            //q[2] = thetta
            //q[3] = s
            //q[4] = v
            //q[6] = w
            //ctrl[0] = fi
            //ctrl[1] = lambda
            double x = q[0];
            double y = q[1];
            double tta = q[2];
            double s = q[3];
            double v = q[4];
            double w = q[5];
            double phi = ctrl[0];
            double lmd = ctrl[1];

            double cosT = Math.Cos(tta);
            double sinT = Math.Sin(tta);
            double cosP = Math.Cos(phi);
            double sinP = Math.Sin(phi);
            double cosTP = Math.Cos(tta + phi);
            double sinTP = Math.Sin(tta + phi);
            //_________________|___________________|_______________|____________|
            double mt_frnt_x = s * cosT - v * sinT - w * c1 * sinT;
            double mt_frnt_y = s * sinT + v * cosT + w * c1 * cosT;
            double mt_rear_x = s * cosT - v * sinT + w * c2 * sinT - lmd * cosT;
            double mt_rear_y = s * sinT + v * cosT - w * c2 * cosT - lmd * sinT;

            double mt_frnt_length = Math.Sqrt(mt_frnt_x * mt_frnt_x + mt_frnt_y * mt_frnt_y);
            double f1 = mt_frnt_length * mu1;
            if (Math.Abs(f1) > f1_max) f1 = f1_max * Math.Sign(f1);

            double mt_rear_length = Math.Sqrt(mt_rear_x * mt_rear_x + mt_rear_y * mt_rear_y);
            double f2 = mt_rear_length * mu2;
            if (Math.Abs(f2) > f2_max) f2 = f2_max * Math.Sign(f2);

            double f1_x = 0;
            double f1_y = 0;
            if (mt_frnt_length != 0)
            {
                f1_x = mt_frnt_x * mu1 * Math.Abs(f1 / mt_frnt_length);
                f1_y = mt_frnt_y * mu1 * Math.Abs(f1 / mt_frnt_length);
            }

            double f2_x = 0;
            double f2_y = 0;
            if (mt_rear_length != 0)
            {
                f2_x = mt_rear_x * mu2 * Math.Abs(f2 / mt_rear_length);
                f2_y = mt_rear_y * mu2 * Math.Abs(f2 / mt_rear_length);
            }

            double f1_sw = +f1_x * cosTP + f1_y * sinTP;
            double f1_vw = -f1_x * sinTP + f1_y * cosTP;

            double f2_s = +f2_x * cosT + f2_y * sinT;
            double f2_v = -f2_x * sinT + f2_y * cosT;

            double f1_s = -f1_vw * Math.Sin(phi);
            double f1_v = +f1_vw * Math.Cos(phi);

            double ds = (f1_s + f2_s) / M;
            double dv = (f1_v + f2_v) / M;
            double dw = (c1 * f1_v - c2 * f2_v) / J;

            double dx = s * cosT - v * sinT;
            double dy = s * sinT + v * cosT;
            double dtta = w;

            qd[0] = dx;
            qd[1] = dy;
            qd[2] = dtta;
            qd[3] = ds;
            qd[4] = dv;
            qd[5] = dw;
        }
    }
}
